#include <CParticles.h>
#include <CUtil.h>


/*
  Initializes and sets up particle populations
 */
void CParticles::Init(int pno, CVector ppos, CVector ppos_spread,  CVector pcolor, CVector pcolor_spread, double BoxSize, int GridSize, double dmin, C3DField* SF) {
  count = pno;
  pos = ppos;
  pos_spread = ppos_spread;
  color = pcolor;
  color_spread = pcolor_spread;
  //type_start = ptype_start;

  SmoothField = SF;

  CVector min(dmin,dmin,dmin); 
  double b = BoxSize;

  Rho.Initialize(GridSize, min, min*-1,BoxSize);
  RhoF.Initialize(GridSize, min, min*-1,BoxSize);
  Fx.Initialize(GridSize, min, min*-1,BoxSize);
  Fy.Initialize(GridSize, min, min*-1,BoxSize);
  Fz.Initialize(GridSize, min, min*-1,BoxSize);

  Maintain();

  enableLensShader = false;
#ifndef NO_OPENGL
  lensShader.Initialize("LensShader");
#endif
}

void CParticles::InitializePhysics() {
  for (int i=0;i<objects.size();i++)
    //   objects[i].Mass = Cosmology->Rho0*pow(Cosmology->ProperUniverseSize,3)/(double)objects.size();
    objects[i].Mass = 1.0;
}

// Return the maximum velocity
void CParticles::FindMaxVelocity() {
  MaxVelocity=1.0;
  for (int i=0;i<objects.size();i++) {
    if (objects[i].V.Length()>MaxVelocity)
      MaxVelocity = objects[i].V.Length();
  }

}


/*
  Integrate particles. Leapfrog or RK4 method
 */
void CParticles::Move(double time) {

  //cout <<"z: " << 1.0/scale -1   <<  " Boxsize:" << Cosmology->ProperUniverseSize*scale/Cosmology->Mpc   << endl;
  //  cout <<"Total mass: " << totalmass << endl;

  for (int i=0;i<objects.size();i++) {

    if (Parameters->IntegratorMethod == 1) // rk4
      objects[i].RungeKuttaIntegrator(time, 1.0);
    if (Parameters->IntegratorMethod == 2) // leapfrog
      objects[i].LeapFrog(time);
    
    // Periodic boundary conditions
    CVector* P = &objects[i].P;
    double s = 0.5*Cosmology->NormalizedUniverseSize;
    double s2 = s*2;
    bool ff = false;
    if (P->x>s) {ff=true; P->x -= s2; }
    if (P->x<=-s) {ff=true; P->x += s2;}
    if (P->y>s) {ff=true; P->y -= s2;}
    if (P->y<=-s) {ff=true; P->y += s2;}
    if (P->z>s) {ff=true; P->z -= s2;}
    if (P->z<=-s) {ff=true; P->z += s2;}
  }
  /*  totalmass = 0;
  for (int i=0;i<objects.size();i++)
    totalmass += objects[i].Mass;
  */
}

CParticle CParticles::InitializeObject() {
  CParticle p;
  p.P = pos + pos_spread.RandomUniform();
  p.Charge = 0.01;
  p.Mass =  1.0; //Util::RandomUniform()*0.5 + 0.5;
  return p;
}



void CParticles::SaveText(string Filename) {
  

}
void CParticles::LoadText(string Filename){

}

/*
  Save binary particle filed
 */
void CParticles::SaveBin(string Filename) {
  ofstream f(Filename.c_str(), ios::out | ios::binary);
  
  unsigned int count = objects.size();
  particle_save* block = new particle_save[count];

  cout << "Writing " << count << " particles " << endl;
  for (int i=0;i<count;i++) {
    block[i].P = objects[i].P;
    block[i].V = objects[i].V;
  }

  f.write((char *)&count, sizeof(unsigned int));
  f.write((char *)block, sizeof(particle_save)*count);

  f.close();
  delete[] block;

 
}
/*
  Load binary particle filed
 */
void CParticles::LoadBin(string Filename){
  ifstream f(Filename.c_str(), ios::out | ios::binary);
  
  cout << "reading file..." << endl;
  unsigned int count = -1;
  f.read((char *)&count, sizeof(unsigned int));
  cout << "count " << count << endl;
  particle_save* block = new particle_save[count];
  f.read((char *)block, sizeof(particle_save)*count);


  objects.clear();
  objects.resize(count);

  cout << "Copying blocks.." << endl;
  for (int i=0;i<count;i++) {
    objects[i].P = block[i].P;
    objects[i].V = block[i].V;
    //objects[i].radius = 1.0;
  }

  cout << "Done loading!" << endl;

  f.close();
  delete[] block;
}

/*
  Load particles from gadget 2.0. NOTE: Velocity not yet loaded properly! Also, size of box must be 50mpc! Currently
  hardcoded, change
 */
void CParticles::LoadGadget(string Filename) {
  ifstream f(Filename.c_str(), ios::out | ios::binary);
  unsigned int npart, dummy;
  
  /*  f.read((char *)&npart, sizeof(unsigned int));
  */
  io_header_1 h;

  f.read((char *)&dummy, sizeof(dummy));
  f.read((char *)&h, sizeof(io_header_1));
  f.read((char *)&dummy, sizeof(dummy));  

  int tot = 0;
  for (int i=0;i<6;i++)
    tot+=h.npart[i];

  cout << "Loading particles: " << tot << endl;
  
  objects.clear();
  objects.resize(tot);
  int pos = 0;
  float P[3];
  for(int k=0;k<6;k++)
    {
      for(int n=0;n<h.npart[k];n++)
	{
	  f.read((char *)&P, sizeof(float)*3);
	  objects[pos].P = CVector(P[0],P[1],P[2])/50000.0 - CVector(0.5,0.5,0.5);
	  pos++;
	}
    }
  pos = 0;
  /* MÅ FIKSES! */
  for(int k=0;k<6;k++)
    {
      for(int n=0;n<h.npart[k];n++)
	{
	  f.read((char *)&P, sizeof(float)*3);
	  objects[pos].V = CVector(P[0],P[1],P[2])/(Cosmology->UniverseSize*1000);// - CVector(0.5,0.5,0.5);
	  // FORCE ZERO
	  //objects[pos].V = CVector(0,0,0);
	  pos++;
	}
    }



  f.close();
  
  //  exit(1);
}





void CParticles::LoadMLAPASCII(string Filename) {
  ifstream f(Filename.c_str(), ios::in);
  
  cout << "reading RAMSES file..." << endl;

  
  char s[2000];
  string er = "";
  f.getline(s,2000);
  count = strtod(s,0);
  
  int MAX = 5000000;
  
  int skipMax = count/MAX;
  if (count > MAX) count = MAX;

  objects.clear();
  objects.resize(count);

  int cnt = 0;
  cout << "Loading.." << endl;

  int cur = 0;

  // skipcounter
  int skip = 0;
  while(!f.eof() && cnt<count) {

    int t  = (int)(cnt*100.0/(float)count);
    if (cur!=t) {
      if (t % 2 == 0)
	cout << " ( " << t << " % ) "<< endl; 
      cur = t;
    }
    
    f.getline(s,2000);
    vector<string> tok;
    
    if (skip--<=0) {
      skip = skipMax;

    CUtil::Tokenize(s, tok," ");
      if (tok.size()==6) {
	strtod(tok.at(0).c_str(),0);
	objects[cnt].P.Set(strtod(tok.at(0).c_str(),0),	strtod(tok.at(1).c_str(),0),	strtod(tok.at(2).c_str(),0));
	objects[cnt].V.Set(strtod(tok.at(3).c_str(),0),	strtod(tok.at(4).c_str(),0),	strtod(tok.at(5).c_str(),0));
	cnt++;
      }
    }
  }
  objects.resize(cnt);
  cout << count << "   and " << cnt << endl;
  cout << endl << "done loading " << count << " particles" << endl;
  f.close();     
}


/*
  Apply a particle object to the current class.
 */

void CParticles::ApplyField(CParticles* F, int ParticleMethod){
  if (ParticleMethod==2) { // brute force
    //Gravity_n2(Parameters.TimeStep);
  }
  
  if (ParticleMethod==0) { // treecode
    Refine();
    Gravity_1(0);
  }
  
  if (ParticleMethod==1) { // particlemesh
    Gravity_Field(F->Fx,F->Fy,F->Fz);
  }


  if (ParticleMethod==3) { // TreePM, currently NOT WORKING
    RhoF.Differentiate(Fx, Fy, Fz,0.05);
    Gravity_Field(F->Fx,F->Fy,F->Fz);
    ApplyTree();
  }
  
}
/*
  Apply the damping term in the end (-V)
 */
void CParticles::ApplyExpansion() {

  for (int i=0;i<objects.size();i++)
    objects[i].A = objects[i].A - objects[i].V*(3.0 + Cosmology->HubbleDeriv(Cosmology->A)/Cosmology->Hubble(Cosmology->A))/Cosmology->A;

}

// Refines the tree
void CParticles::Refine() {
  /*  tree.Kill();
    CVector max = CVector(-10E30, -10E30, -10E30);
  CVector min = CVector(10E30, 10E30, 10E30);
  for (int i=0;i<objects.size();i++) {
    if (objects[i].P.x>max.x) max.x = objects[i].P.x;
    if (objects[i].P.y>max.y) max.y = objects[i].P.y;
    if (objects[i].P.z>max.z) max.z = objects[i].P.z;

    if (objects[i].P.x<min.x) min.x = objects[i].P.x;
    if (objects[i].P.y<min.y) min.y = objects[i].P.y;
    if (objects[i].P.z<min.z) min.z = objects[i].P.z;
  }
  CVector diff = max-min;
  */
  boundsize = abs(diff.x);
  if (abs(diff.y)>boundsize) boundsize = abs(diff.y);
  if (abs(diff.z)>boundsize) boundsize = abs(diff.z);
  boundsize = boundsize / 1.99;
  center = (max + min)*0.5;
  

  center = CVector(0,0,0);
  boundsize = Cosmology->UniverseSize*0.5;

  for (int i=0;i<objects.size();i++) {
    tree.Refine(center, boundsize, &objects[i]);
  }

}

// Tree-only gravity. Currentlt not USABLE!
void CParticles::Gravity_n2(double time) {
    CVector tmp = CVector(0,0,0);
    int cnt = 0;
    double g = Cosmology->Gravity;
    
        for (int i1=0;i1<objects.size();i1++) {
      objects[i1].A = CVector(0,0,0);
      CVector f = CVector(0,0,0);
      for (int j1=0;j1<objects.size();j1++) {
	if (i1!=j1) {

	  for (int pp = 1;pp<2;pp++)
	    for (int i=0;i<3;i++)
	    for (int j=0;j<3;j++)
	      for (int k=0;k<3;k++) {
		{
		  int b  = Cosmology->UniverseSize;
		  CVector P = objects[j1].P;
		   P  = objects[j1].P + CVector(i-1, j-1, k-1)*(b);

		  CVector F = ((P+objects[i1].P*-1))*Cosmology->A;
		  double r = F.Length();
		  CVector Force = F/r;
		  double e = 5;
		  
		  if (r<Cosmology->UniverseSize*Cosmology->A)
		    {
		      Force = Force * Cosmology->Gravity/(r*r + e*e);
		      objects[i1].A = objects[i1].A + Force*objects[j1].Mass;
		    }
		}
	      }

	}
	  //	  f = f + objects[i].CalculateGravity(objects[j], 500);
	}
    
      //      objects[i].A = f/objects[i].Mass;
      
      // if (i1==0) cout << objects[i1].A;
      //if (i1==0) cout << "count" << cnt << "  -  tmp: " << tmp;
      
      /*      glPushMatrix();
      glTranslatef(objects[i1].P.x, objects[i1].P.y, objects[i1].P.z);
      glColor4f(1,0,1,1);
      glBegin(GL_LINES);
      glVertex3f(0,0,0);
      CVector F = objects[i1].A*5;
      if (F.Length()>50) F=F.Normalize()*50.0;
      if (F.Length()<2) F=F.Normalize()*2.0;
      glVertex3f(F.x,F.y, F.z);
      
      glEnd();
      glPopMatrix();
      */
    }
}

// N^2 gravity. Currenrlt NOT USABLE
void CParticles::Gravity_1(double tim) {
        int sum = 0;
    double M = 0;
    int cnt = 0;
    for (int l=0;l<objects.size();l++) {
      //objects[i].A = CVector(0,0,0);

      CVector P = objects[l].P;
      for (int i=1;i<2;i++)
	for (int j=1;j<2;j++)
	  for (int k=1;k<2;k++) {
	    int t = 0;
	    objects[l].P = P + CVector(i-1, j-1, k-1)*Cosmology->UniverseSize;
	    tree.Interact(&objects[l],Parameters->ThetaAngle, t);
	    if (i==k==j==1)
	      cnt = t;
	  }
    
      objects[l].P = P;


      sum+=cnt;
      M += objects[l].Mass;
    }
    cout <<"Interacting particles average: " << sum/objects.size() << endl;
  }


/*
  Apply the tree forces
 */
void CParticles::ApplyTree() {
  int sum = 0;
  double M = 0;
  int cnt = 0;
  for (int l=0;l<objects.size();l++) {
    int t = 0;
    tree.Interact(&objects[l],Parameters->ThetaAngle, t);
    cnt = t;
    sum+=cnt;
    M += objects[l].Mass;
  }
  cout <<"Interacting particles average: " << sum/objects.size() << endl;
}


// Not used currently
void CParticles::BoundaryBox(double size, double time, double str) {
    return;
    /*    for (int i=0;i<objects.size();i++) {
	CVector F = CVector(objects[i].P.x, objects[i].P.y, objects[i].P.z);
	double l = F.Length();
	double s = boxsize;
	//if (l>s) l = s;
        l = s -l;
	if (l<10) l = 10;
	F = F.Normalize();
	//if (l<size/2)
	F = F/(l*l) * totalmass*1.0*40000*0;
	
	objects[i].A = objects[i].A + F;
      }


    return;
    */
    for (int i=0;i<objects.size();i++) {
      for (int j=0;j<6;j++) {
	CVector m = CVector(0,0,0);
	if (j==0) 
	  m = CVector(1,0,0);
	if (j==1) 
	  m = CVector(0,1,0);
	if (j==2) 
	  m = CVector(0,0,1);
	if (j==3) 
	  m = CVector(1,1,0);
	if (j==4) 
	  m = CVector(0,1,1);
	if (j==5) 
	  m = CVector(1,0,1);

	CVector F = CVector(objects[i].P.x*m.x, objects[i].P.y*m.y, objects[i].P.z*m.z);
	
	double l = F.Length();
        F = F/l;
	
	F = F*l * objects[i].Mass*totalmass*0.006*0;
	
	objects[i].A = objects[i].A + F;
      }
    }
  }


/*
  Initialize particles from a matter field
 */
void CParticles::InitializeFromField(C3DField& Field) {

  C3DField Fx, Fy, Fz;
  Field.Differentiate(Fx,Fy,Fz);

  for (int i=0;i<objects.size();i++) {
    CVector p(Field.Width,Field.Width,Field.Width);
    p = p.RandomUniform()+ CVector(0.5*Field.Width,0.5*Field.Width,0.5*Field.Width);
    //p = CVector((int)p.x, (int)p.y, (int)p.z);
    double boxsize = Cosmology->NormalizedUniverseSize;
    CVector RealPos = p*boxsize/(double)Field.Width - CVector(boxsize/2.0,boxsize/2.0,boxsize/2.0);
    // they are now placed somewhere
    CVector v;
    //cout <<p << endl;
    v.x= Fx.GetR((int)p.x, (int)p.y, (int)p.z);
    v.y= Fy.GetR((int)p.x, (int)p.y, (int)p.z);
    v.z= Fz.GetR((int)p.x, (int)p.y, (int)p.z);
    objects[i].P = RealPos  - v;
    objects[i].V = v*-1;
    objects[i].A = 0;
  }

}

